#include<opencv2/opencv.hpp>
#include<X11/Xlib.h>
#include<iostream>
#include"GxCamera/GxCamera.h"
#include"AngleSolver/AngleSolver.h"
#include"WindMill/Energy.h"
#include"CAN/can.h"

using namespace cv;
using namespace std;

pthread_t thread1;
pthread_t thread2;
void* imageUpdatingThread(void* PARAM);
void* detectingThread(void* PARAM);

Mat srcFrame = Mat::zeros(480,640,CV_8UC3);
pthread_mutex_t Globalmutex;
pthread_cond_t GlobalCondCV;
bool imageReadable = false;
bool newFrame=true;
void* param;

//import Galaxy Camera
GxCamera camera;

//import angle solver
AngleSolver angleSolver;
double yaw, pitch, measureDistance;

//import Windmill Detector
Energy windmillDetector;
Point2f centerPoint;
bool isFoundArmor;

int main(int argc, char** argv)
{
    //For MutiTHread
    XInitThreads();
    //Init mutex
    pthread_mutex_init(&Globalmutex,NULL);
    //Init cond
    pthread_cond_init(&GlobalCondCV,NULL);
    //Create thread 1 -- image acquisition thread
    pthread_create(&thread1,NULL,imageUpdatingThread,NULL);
    //Create thread 2 -- armor Detection thread
    pthread_create(&thread2,NULL,detectingThread,NULL);
    //Wait for children thread
    pthread_join(thread1,NULL);
    pthread_join(thread2,NULL);
    pthread_mutex_destroy(&Globalmutex);
    return 0;
}


void* imageUpdatingThread(void* PARAM)
{
    //init camrea lib
    camera.initLib();

    //   open device      SN号
    camera.openDevice("KJ0190120004");

    //Attention:   (Width-64)%2=0; (Height-64)%2=0; X%16=0; Y%2=0;
    //   ROI             Width           Height       X       Y
    camera.setRoiParam(   640,            480,        80,     120);

    //   ExposureGain          autoExposure  autoGain  ExposureTime  AutoExposureMin  AutoExposureMax  Gain(<=16)  AutoGainMin  AutoGainMax  GrayValue
    camera.setExposureGainParam(    false,     true,      2000,          1000,              2000,         16,         15,            16,        127);

    //   WhiteBalance             Applied?       light source type
    camera.setWhiteBalanceParam(    true,    GX_AWB_LAMP_HOUSE_ADAPTIVE);

    //   Acquisition Start!
    camera.acquisitionStart(&srcFrame);
}

void* detectingThread(void* PARAM)
{
    angleSolver.setCameraParam("/home/dji/Documents/JLURoboVision/General/camera_params.xml", 1);
    angleSolver.setBulletSpeed(15000);
    measureDistance = 4000;
    angleSolver.setDistance(measureDistance); //在此假定为4M
    usleep(1000000);

    char chKey;
    bool bRun = true;

    do
    {
        Mat myFrame;
        centerPoint = Point2f(403,177);
        //consumer gets image
        pthread_mutex_lock(&Globalmutex);
        while (!imageReadable) {
            pthread_cond_wait(&GlobalCondCV,&Globalmutex);
        }
        if(newFrame)
            srcFrame.copyTo(myFrame);
        imageReadable = false;
        pthread_mutex_unlock(&Globalmutex);

        // 大风车识别
        windmillDetector.run(srcFrame);
        // 是否找到目标
        //isFoundArmor = energy.isFoundArmor;
        isFoundArmor = true;
        if(isFoundArmor)
        {
            // 获取大风车识别的目标中心点
            centerPoint = windmillDetector.target_point;
        }
        // 角度解算获取yaw、pitch角
        angleSolver.getAngle(centerPoint, yaw, pitch);
        Can(yaw,pitch,measureDistance,true,isFoundArmor);

        // from here you can use mat image as the name 'myFrame'
        imshow("Frame",myFrame);

        chKey = waitKey(1);
        switch (chKey) {
        case 'p':
        case 'Q':
        case 27:
            bRun = false;
            break;
        default:
            break;
        }
    } while (bRun);
}
